#include "timer.h"

u32 ms10 = 0; //时间总线系数 
u16 SPL06_Scan =0;
uint16_t disable_times=0;
float pressure, temperature,asl;



/*****************************************************************************
*函  数：void TIM2_Init(void)
*功  能：TIM2初始化为10ms计数一次
*参  数：无
*返回值：无
*备  注：更新中断时间 Tout = (ARR-1)*(PSC-1)/CK_INT
*****************************************************************************/
void TIM2_Init(void)
{
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM2,ENABLE);
	
  //TIM2~TIM5时钟来源于APB1*2
  TIM_TimeBaseStructure.TIM_Period = 10000-1; //自动重装载值
  TIM_TimeBaseStructure.TIM_Prescaler = 84-1; //预分频值
  TIM_TimeBaseStructure.TIM_ClockDivision = 0; 
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM2, &TIM_TimeBaseStructure);
	       
	TIM_ITConfig(TIM2,TIM_IT_Update,ENABLE);
	
  TIM_Cmd(TIM2,ENABLE);
}

/*****************************************************************************
*函  数：void TIM4_Init(void)
*功  能：TIM4初始化为10ms计数一次
*参  数：无
*返回值：无
*备  注：更新中断时间 Tout = (ARR-1)*(PSC-1)/CK_INT
*****************************************************************************/
void TIM3_Init(void)
{
  TIM_TimeBaseInitTypeDef  TIM_TimeBaseStructure;
	
  RCC_APB1PeriphClockCmd(RCC_APB1Periph_TIM3,ENABLE);
	
  //TIM2~TIM5时钟来源于APB1*2
  TIM_TimeBaseStructure.TIM_Period = 10000-1; //自动重装载值
  TIM_TimeBaseStructure.TIM_Prescaler = 84-1; //预分频值
  TIM_TimeBaseStructure.TIM_ClockDivision = 0; 
  TIM_TimeBaseStructure.TIM_CounterMode = TIM_CounterMode_Up;
  TIM_TimeBaseInit(TIM3, &TIM_TimeBaseStructure);
	       
	TIM_ITConfig(TIM3,TIM_IT_Update,ENABLE);
	
  TIM_Cmd(TIM3,ENABLE);
}

/****************************************************************************************************
*函  数: void TIM2_IRQHandler(void) 
*功  能: TIM2定时器中断，10ms进一次中断也就是100Hz
*参  数: 无
*返回值: 无
*备  注: 此函数是整个程序的运行时基，不同的中断时间对应不同频率，
*        对于一些计算对调用时间要求比较严格时可用此方法；
*        扫描频率 = 100Hz/分频系数；
****************************************************************************************************/
float in_time,out_time,delay_1,time_flag=1;//测时间
uint16_t led_turnover = 0;
uint8_t MAVLink_buf[57];
uint16_t MAVLink_Length;
uint32_t MAVLink_Time;
void TIM2_IRQHandler(void) 
{
	if(TIM_GetITStatus(TIM2,TIM_IT_Update) != RESET)
	{
		////////////计时时间控制/////////////
		ms10++;
		if(ms10-PPM_Last_recv_time > 50)//已经有0.5s没收到接收机信号了
		{
			PPM_Rec_Okay=0;
		}
		else PPM_Rec_Okay=1;
		
		//正常运行 LED 每0.5S翻转一次
		if(ms10%50 == 0) led_turnover++;
		if (led_turnover%2 == 0) RGB_LED_Blue();
		else RGB_LED_Green();
		/**********************传感器采样与控制***********************/
		IMU(); //姿态解算
		Control();

	/**********************MAVLink协议发送***********************/	

//		MAVLink_Time=(uint32_t)(10*ms10+TIM2->CNT/1000);
//		Msg32_LOCAL_POSITION_NED(MAVLink_buf,&MAVLink_Length,MAVLink_Time,position_ned_x,position_ned_y,position_ned_z,position_ned_Vx,position_ned_Vy,position_ned_Vz); 
//		Usart_SendNByte(UART4,MAVLink_buf,MAVLink_Length);
		

		if(PPM_Is_Okay==0||PPM_Rec_Okay==0) 
		{
			Uav_Enable=0;
			//printf("conmunication lost!!!\n");
		}
		else
		{
			if(PPM_Databuf[6]>800&&PPM_Databuf[6]<1200) 
			{
				Uav_Enable=1;//无人机解锁
			}
			else 
			{
				Uav_Enable=0;//无人机上锁
				disable_times++;
			}
			if (disable_times>500)
			{
				disable_times=0;
				printf("UAV DISABLE\n");
			}
		}
	}
	TIM_ClearITPendingBit(TIM2,TIM_IT_Update);
}


void Data_Check(void)
{
	while(Position.X==0||Position.Y||V.X==0||V.Y==0||Att_Angle.pit==0||Att_Angle.rol==0) {}
}
